// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GraphOptimizationAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.Localization
{
    using System;
    using System.Collections.Concurrent;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.Serialization;
    using System.Threading;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Performs graph optimization on odometry and generates a corrected recording. 
    /// Once everything is processed, the agent outputs corrected drive feedback messages and the corresponding HDP messages.
    /// Thus a SLAM agent can be hooked both before and after this agent, each with varying degrees of processing.
    /// </summary>
    /// <typeparam name="T">A kind of horizontal depth profile</typeparam>
    /// <typeparam name="TDriveFeedbackState">A kind of drive feedback state</typeparam>
    [DataContract]
    public class GraphOptimizationAgent<T, TDriveFeedbackState> :
        SynchronizedConsumerProducerAgent<SLAM2DAgentMessage<TDriveFeedbackState>, SimpleAgentMessage<T>, SimpleAgentMessage<PoseFeedbackState>>,
        ISubscriptionReceiver<GamePadAgentMessage>, 
        IMessagePublisher<SimpleAgentMessage<T>>
        where T : IHorizontalDepthProfile
        where TDriveFeedbackState : IDriveState
    {
        /// <summary>
        /// The name of the gamepad producer. Loop closure is driven by the gamepad controller for now (Y button)
        /// </summary>
        [DataMember]
        private string gamepadProducerName;

        /// <summary>
        /// The name of the slam producer. 
        /// </summary>
        [DataMember]
        private string slamProducerName;

        /// <summary>
        /// The name of the hdp producer. We are a pass-through for these messages.
        /// </summary>
        [DataMember]
        private string hdpProducerName;

        /// <summary>
        /// The gamepad producer. Loop closure is driven by the gamepad controller for now (Y button)
        /// </summary>
        private IAgentHost gamepadProducer;

        /// <summary>
        /// The publishing port for hdp messages
        /// </summary>
        private IForwarder<SimpleAgentMessage<T>> hdpPublisher;

        /// <summary>
        /// The collection of drive messages to process
        /// </summary>
        private List<Pose2D> poses;

        /// <summary>
        /// The collection of hdp messages to store and publish. 
        /// Note: we might need to persist these instead of keeping them in memory.
        /// </summary>
        private List<SimpleAgentMessage<T>> hdps;

        /// <summary>
        /// Tracks the last SLAM originating time we've seen
        /// </summary>
        private long loopClosureTime;

        /// <summary>
        /// Initializes a new instance of the GraphOptimizationAgent class. 
        /// This constructor is used in manifest creation, but is not used at runtime.
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="slamProducer">The SLAM producer</param>
        /// <param name="hdpProducer">The hdp producer</param>
        /// <param name="gamepadProducer">The gamepad producer</param>
        public GraphOptimizationAgent(
            string name,
            IProducer<SLAM2DAgentMessage<TDriveFeedbackState>> slamProducer,
            IProducer<SimpleAgentMessage<T>> hdpProducer,
            IProducer<GamePadAgentMessage> gamepadProducer)
            : base(name, 5)
        {
            this.gamepadProducerName = gamepadProducer.Name;
            this.hdpProducerName = hdpProducer.Name;
            this.slamProducerName = slamProducer.Name;
        }

        /// <summary>
        /// The name of the SLAM agent
        /// </summary>
        protected override string ProducerName1
        {
            get { return this.slamProducerName; }
        }

        /// <summary>
        /// The name of the Depth Profile agent
        /// </summary>
        protected override string ProducerName2
        {
            get { return this.hdpProducerName; }
        }

        /// <summary>
        /// Initializer, in lieu of a constructor
        /// </summary>
        /// <param name="locator">The locator that can provide access to other agents</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.gamepadProducer = locator.GetAgent(this.gamepadProducerName);
            this.gamepadProducer.Subscribe<GamePadAgentMessage>(this);
        }

        /// <summary>
        /// Called to store the forwarder for the given type we want to publish
        /// </summary>
        /// <param name="forwarder">The forwarder to use when publishing</param>
        public void InitializePublisher(IForwarder<SimpleAgentMessage<T>> forwarder)
        {
            this.hdpPublisher = forwarder;
        }

        /// <summary>
        /// Cascade-activate the producers
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.gamepadProducer.Activate();
            this.poses = new List<Pose2D>();
            this.hdps = new List<SimpleAgentMessage<T>>();
        }

        /// <summary>
        /// Cascade-deactivate the producers
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.gamepadProducer.Deactivate();
            this.poses = null;
            this.hdps = null;
            this.loopClosureTime = 0;
        }

        /// <summary>
        /// When the Y button is pressed signaling loop closure, this method runs the graph optimization and outputs all messages (for recording)
        /// </summary>
        /// <param name="message">The gamepad message</param>
        public void Receive(GamePadAgentMessage message)
        {
            // if the loop closure button is pressed, record the time and kick off processing
            if (message.Buttons.Y && this.loopClosureTime == 0)
            {
                this.loopClosureTime = message.OriginatingTime;
                this.Optimize();
            }
        }

        /// <summary>
        /// Receives and stores the location from slam messages and the hdp messages for later processing
        /// </summary>
        /// <param name="message1">The slam message to correct</param>
        /// <param name="message2">The hdp message to store and forward</param>
        protected override void Receive(SLAM2DAgentMessage<TDriveFeedbackState> message1, SimpleAgentMessage<T> message2)
        {
            if (this.hdps != null)
            {
                this.poses.Add(message1.Location);
                this.hdps.Add(message2);
            }
        }

        /// <summary>
        /// Performs the optimization and publishes the results
        /// </summary>
        private void Optimize()
        {
            Console.WriteLine("Optimizing");

            // process the poses
            Pose2D[] optimizedPoses = GraphOptimization.OptimizeGraphForPoseSlamSimpleLoop(this.poses.ToArray(), true, false, false);

            // transform the poses into drive messages and combine them with the HDPs (we know we have the same number of each, since we synchronized them)
            IEnumerable<Tuple<SimpleAgentMessage<PoseFeedbackState>, SimpleAgentMessage<T>>> resultSet = optimizedPoses.Zip(
                this.hdps,
                (pose, hdp) =>
                {
                    PoseFeedbackState pfs = new PoseFeedbackState(pose, new PoseNoiseModelParams());                                                            
                    
                    return Tuple.Create(new SimpleAgentMessage<PoseFeedbackState>(pfs, hdp.OriginatingTime), new SimpleAgentMessage<T>(hdp.State, hdp.OriginatingTime));
                });

            // go over the set and publish each pair
            foreach (Tuple<SimpleAgentMessage<PoseFeedbackState>, SimpleAgentMessage<T>> result in resultSet)
            {
                this.Publisher.Post(result.Item1, true);
                this.hdpPublisher.Post(result.Item2, true);
            }

            // set the lists to null to avoid reprocessing and to free the memory
            this.hdps = null;
            this.poses = null;
            Console.WriteLine("Optimization complete.");
        }
    }
}
